/* Standard includes. */
#include <stdlib.h>

/* OS includes. */
#include "AT91SAM7X256.h"
#include "lib_AT91SAM7X256.h"

/* Flying Fox includes */
#include "ffox.h"
#include "analogin.h"
#include "serial.h"
#include "servo.h"
#include "i2c.h"
#include "hp03.h"
#include "timer.h"
#include "receiver.h"
#include "navigate.h"
#include "usb.h"
#include "sensor.h"
#include "dbgprintf.h"


/* Priorities for the demo application tasks. */
#define MAIN_TASK_PRIORITY      ( tskIDLE_PRIORITY + 3 )
#define SENSOR_TASK_PRIORITY    ( tskIDLE_PRIORITY + 4 )

/*
 * Configure the processor for use with the Atmel demo board.  Setup is minimal
 * as the low level init function (called from the startup asm file) takes care
 * of most things.
 */
static void SetupHardware( void );


/*
 * Starts all the other tasks, then starts the scheduler. 
 */
int main( void )
{
	/* get the processor on its feet */
	AT91F_LowLevelInit();
	
	/* Setup any hardware that has not already been configured by the low
	level init routines. */
	SetupHardware();
	
    /* start the sensor aquisition tasks */
	Sensor_StartTasks(SENSOR_TASK_PRIORITY);

    /* start the flight navigation software */
	Navigate_StartTasks(MAIN_TASK_PRIORITY);
    
	/* Start the scheduler */
	vTaskStartScheduler();

	/* We should never get here as control is now taken by the scheduler. */
  	return 0;
}

/*-----------------------------------------------------------*/

/*
 * I/O ports for Flying Fox
 *
 * PA0  - output default 1 - LED1, active low
 * PA1  - output default 1 - LED2, active low
 * PA2  - output default 0 - HP03D pressure sensor XCLR
 * PA3  - i/o (i2c)        - I2C SDA data (open drain)
 * PA4  - output (i2c)     - I2C SCL clock (open drain)
 * PA5  - input (uart0)    - GPS UART0 RXD0 serial receive (pullup)
 * PA6  - output (uart0)   - GPS UART0 TXD0 serial transmit (pullup)
 * PA7  - output default 0 - servo1 output
 * PA8  - input            - receiver1 input (pullup)
 * PA9  - input            - receiver2 input (pullup)
 * PA10 - input            - receiver3 input (pullup)
 * PA11 - output (spi)     - 
 * PA12 - input (spi)      - 
 * PA13 - output (spi)     - 
 * PA14 - output (spi)     - 
 * PA15 - output (timer1)  - HP03D pressure sensor MCLK from TIOA1
 * PA16 - output default 0 - servo6 output
 * PA17 - analog input     - leveler input a from AD0
 * PA18 - analog input     - leveler input b from AD1
 * PA19 - analog input     - gyro x? from AD2
 * PA20 - analog input     - gyro y? from AD3
 * PA21 - input            - receiver6 input (pullup) / alternate RXD1 for serial radio modem
 * PA22 - input            - receiver7 input (pullup) / alternate TXD1 for serial radio modem
 * PA23 - output default 0 - servo7 output
 * PA24 - input            - 
 * PA25 - input            - receiver5 input (pullup)
 * PA26 - input            - receiver4 input (pullup)
 * PA27 - output default 0 - servo2 output
 * PA28 - output default 0 - servo3 output
 * PA29 - output default 0 - servo4 output
 * PA30 - output default 0 - servo5 output
 * PA31 - input            - USB detect (schottky?) active high
 */
 
static void SetupHardware( void )
{
	/* When using the JTAG debugger the hardware is not always initialised to
	the correct default state.  This line just ensures that this does not
	cause all interrupts to be masked at the start. */
	AT91C_BASE_AIC->AIC_EOICR = 0;
	
	/* Most setup is performed by the low level init function called from the 
	startup asm file. */

	/* set digital inputs and outputs */
	AT91F_PIO_CfgInput(AT91C_BASE_PIOA, RECEIVER_MASK | USB_DETECT);
	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, LED_MASK | HP03D_XCLR | SERVO_MASK);

	/* set up non-PIO peripherals */
	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 
	                I2C_SDA | I2C_SCL | GPS_RXD | GPS_TXD | SRADIO_RXD | SRADIO_TXD, 0);
	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOB, HP03D_MCLK, 0);
	
	/* set LEDs and I2C as open drain outputs */
	AT91F_PIO_CfgOpendrain(AT91C_BASE_PIOA, LED_MASK | I2C_SDA | I2C_SCL );
	
	/* set initial PIO state */
	AT91F_PIO_SetOutput(AT91C_BASE_PIOA, LED_MASK);
	AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, HP03D_XCLR | SERVO_MASK);
	
	/* set pullups */
	AT91F_PIO_CfgPullup(AT91C_BASE_PIOA, 
		GPS_RXD | SRADIO_RXD | 
		RECEIVER1_INPUT | RECEIVER2_INPUT | RECEIVER3_INPUT | RECEIVER4_INPUT | 
		RECEIVER5_INPUT );

	/* Enable the clocks */
	AT91F_PMC_CfgSysClkEnableReg(AT91C_BASE_PMC, AT91C_PMC_UDP);
	AT91F_PMC_EnablePeriphClock(AT91C_BASE_PMC, (1 << AT91C_ID_PIOA) | 
	                                            (1 << AT91C_ID_PIOB) | 
	                                            (1 << AT91C_ID_ADC) |
	                                            (1 << AT91C_ID_US0) |
	                                            (1 << AT91C_ID_US1) |
	                                            (1 << AT91C_ID_TWI) |
	                                            (1 << AT91C_ID_UDP) |
	                                            (1 << AT91C_ID_TC0) |
	                                            (1 << AT91C_ID_TC1) |
	                                            (1 << AT91C_ID_TC2) );
	
	/* configure serial ports */
	Serial_Init();
	
	/* enable debugging output */
	dbgprintf_Init();
	
	/* configure the microsecond timer */
	Timer_Init();
	
	/* configure analog inputs */
	AnalogIn_Init();
	
	/* configure servo outputs */
	Servo_Init();
	
	/* configure receiver inputs */
	Receiver_Init();
	
	/* configure I2C */
	I2C_Init();
	
	/* configure altitude sensor */
	HP03_Init();
	
	/* configure USB */
//	USB_Init();
}


/*
 * NAME:        FFox_TaskCreate
 * ACTION:      Creates a task in the same way as FreeRTOS's xTaskCreate, 
 *              except it reports errors in the debug stream
 */
 
void FFox_TaskCreate(pdTASK_CODE pvTaskCode, const signed portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask)
{
    portBASE_TYPE errorCode;
    
    errorCode = xTaskCreate(pvTaskCode, pcName, usStackDepth, pvParameters, uxPriority, pvCreatedTask);
    if (errorCode != pdPASS)
    {
        dbgprintf("ERROR - couldn't create task '%s': error code %ld\r\n", pcName, errorCode);
    }
}


